#!/usr/bin/env python

import rospy
from std_msgs.msg import String
import sys, select, termios, tty

def getKey():
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

if __name__ == "__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('key_publisher', anonymous=True)
    pub = rospy.Publisher('key_input', String, queue_size=10)

    rate = rospy.Rate(10) # 10hz
    print("Press keys to publish. Press 'q' to quit.")
    print("w->forward,s->backward,a->left,d->right")
    try:
        while not rospy.is_shutdown():
            key = getKey()
            print(key)
            if key == 'q':
                break
            pub.publish(key)
            rate.sleep()

    except Exception as e:
        print(e)

    finally:
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
